#include "stop_planner.h"
#include "simple_tp.h"

using axisgroup::StopPlanner;

int StopPlanner::init(RobJointVal cur_pos, RobJointVal cur_vel, int joint_num, double ts)
{
    stop_tp_.clear();
    for (int i = 0; i < joint_num; i++)
    {
        auto tp = std::make_shared<simple_tp_t>();
        stop_tp_.push_back(tp);
        stop_tp_[i]->reset(cur_pos[i]);
        stop_tp_[i]->curr_vel = cur_vel[i];
        stop_tp_[i]->max_acc = 720;  // TODO LW
        stop_tp_[i]->enable = true;
    }
    joint_num_ = joint_num;
    ts_ = ts;
    return 0;
}

int StopPlanner::update(RobJointVal& stop_cmd)
{
    for (int i = 0; i < joint_num_; i++)
    {
        // 更新 tp->curr_pos
        simple_tp_deceleration(stop_tp_[i].get(), ts_);

        //设定目标位置
        stop_cmd[i] = stop_tp_[i]->curr_pos;
    }
    return 0;
}

bool StopPlanner::is_stopped()
{
    for (auto tp : stop_tp_)
    {
        if (tp->active)
        {
            return false;
        }
    }
    return true;
}
